#include <Eigen/Dense>
#include <string>
#include <string.h>
// #include <sys/time.h>
// #include <ctime>
// #include <chrono>
// #include <iomanip>
// #include <stdio.h>
#include <iostream>
#include <cmath>

#include "global.h"
#include "params.h"
using namespace std;
using Eigen::Matrix3d;
using Eigen::Vector3d;

#define L1 0.05
#define L2 0.37
#define L3 0.40
#define MY_PI 3.1415926

// #define TO_DEGREE
// #define IK_TEST
#define ZERO_OFFSET_SPOT // spot狗的角度定义偏移
// #define TO_WORLD

Matrix3d R(string axis, double angle) // 计算x,y,z欧拉旋转矩阵
{
  angle = angle * 3.1415926 / 180.0; // sin函数参数为弧度制，需转换
  Matrix3d mat;
  if (axis == "x") {
    mat << 1, 0, 0, 0, cos(angle), -sin(angle), 0, sin(angle), cos(angle);
  } else if (axis == "y") {
    mat << cos(angle), 0, sin(angle), 0, 1, 0, -sin(angle), 0, cos(angle);
  } else if (axis == "z") {
    mat << cos(angle), -sin(angle), 0, sin(angle), cos(angle), 0, 0, 0, 1;
  }
  return mat;
}

Matrix3d Antisymmetric(Vector3d vec) {
  Matrix3d mat;
  mat << 0, -vec(2), vec(1), vec(2), 0, -vec(0), -vec(1), vec(0), 0;
  return mat;
}

Matrix3d JacobianMatrix(Vector3d vec, int ksi) {
  Matrix3d tmpmat;
  tmpmat(0, 0) = 0;
  tmpmat(0, 1) = -L_2 * cos(vec(1)) - L_3 * cos(vec(1) + vec(2));
  tmpmat(0, 2) = -L_3 * cos(vec(1) + vec(2));
  tmpmat(1, 0) = ksi * L_1 * sin(vec(0)) +
                 L_3 * cos(vec(0)) * cos(vec(1) + vec(2)) +
                 L_2 * cos(vec(1)) * cos(vec(0));
  tmpmat(1, 1) = -L_3 * sin(vec(0)) * sin(vec(1) + vec(2)) -
                 L_2 * sin(vec(1)) * sin(vec(0));
  tmpmat(1, 2) = -L_3 * sin(vec(0)) * sin(vec(1) + vec(2));
  tmpmat(2, 0) = ksi * L_1 * cos(vec(0)) +
                 L_3 * sin(vec(0)) * cos(vec(1) + vec(2)) +
                 L_2 * cos(vec(1)) * sin(vec(0));
  tmpmat(2, 1) = L_3 * cos(vec(0)) * sin(vec(1) + vec(2)) +
                 L_2 * sin(vec(1)) * cos(vec(0));
  tmpmat(2, 2) = L_3 * cos(vec(0)) * sin(vec(1) + vec(2));

  return tmpmat;
}

/**
 * Calculates the inverse kinematics for a given leg and foot position.
 *
 * @param legNumber The number of leg range from 0~3.
 * @param footPosition The position of the foot in Leg system.
 * @return  The joint angles required to achieve the given foot position.
 */
Vector3d inverseKinematics(int legNumber, Vector3d footPosition) {
  Vector3d motorAngles;

  double gamma, alfa, beta;

  double x = footPosition[0];
  double y = footPosition[1];
  double z = footPosition[2];

  double ksi = (legNumber == 1 || legNumber == 3) ? 1 : -1;

  gamma = MY_PI - acos(L1 / sqrt(pow(y, 2) + pow(z, 2))) - atan(-ksi * y / z);

  float L23 = sqrt(pow(x, 2) + pow(y, 2) + pow(z, 2) - pow(L1, 2));

  beta = acos((pow(L2, 2) + pow(L3, 2) - pow(L23, 2)) / (2 * L2 * L3));

  alfa = acos(x / L23) +
         acos((pow(L2, 2) + pow(L23, 2) - pow(L3, 2)) / (2 * L2 * L23));

#ifdef IK_TEST
  cout << "x: " << x << " y: " << y << " z: " << z << endl;
  cout << atan((L1 * cos(gamma) - z) / x) << endl;
  cout << asin(sqrt(pow(L1 * sin(gamma) - y, 2) + pow(L1 * cos(gamma) - z, 2)) /
               L23)
       << endl;

  cout << "gamma: " << gamma << endl;
  cout << "L23: " << L23 << endl;
  cout << "beta: " << beta << endl;
  cout << "alfa: " << alfa << endl;
#endif

#ifdef ZERO_OFFSET_SPOT
  gamma = (legNumber == 1 || legNumber == 3) ? gamma - MY_PI / 2
                                             : MY_PI / 2 - gamma;
  alfa = -alfa + (2 * MY_PI / 3);
  beta = -beta + 1.6;
#endif

#ifdef TO_DEGREE
  gamma = gamma * 180 / MY_PI;
  alfa = alfa * 180 / MY_PI;
  beta = beta * 180 / MY_PI;
#endif

  motorAngles[0] = gamma;
  motorAngles[1] = alfa;
  motorAngles[2] = beta;

  return motorAngles;
}

/**
 * the forward kinematics for a given leg number and joint position.
 *
 * @param legNumber The number of leg range from 0~3.
 * @param jointPosition The position of the joint in the leg in Radian.
 * @return The calculated foot position as a Vector3d object in Leg system.
 */
Vector3d forwardKinematics(int legNumber, Vector3d jointPosition) {
  double gamma = jointPosition[0];
  double alfa = jointPosition[1];
  double beta = jointPosition[2];

#ifdef ZERO_OFFSET_SPOT
  gamma = (legNumber == 1 || legNumber == 3) ? gamma + MY_PI / 2
                                             : MY_PI / 2 - gamma;
  alfa = -alfa + (2 * MY_PI / 3);
  beta = -beta + 1.6;
#endif

  double delta = (legNumber == 0 || legNumber == 1) ? 1 : -1;
  double ksi = (legNumber == 1 || legNumber == 3) ? 1 : -1;

  double pow_L23 = pow(L2, 2) + pow(L3, 2) - 2 * L2 * L3 * cos(beta);

  double x = L2 * cos(alfa) - L3 * cos(alfa + beta);
  double y =
      ksi * sqrt(pow_L23 - pow(x, 2)) * cos(gamma) + ksi * L1 * sin(gamma);
  double z = -sqrt(pow_L23 - pow(x, 2)) * sin(gamma) + L1 * cos(gamma);

  return Vector3d(x, y, z);
}

Vector3d bodyToLeg(int legNumber, Vector3d footPosition) {
  switch (legNumber) {
  case 0: // RF
    return Vector3d(footPosition[0] - H_X, footPosition[1] + H_Y,
                    footPosition[2]);
    break;
  case 1: // LF
    return Vector3d(footPosition[0] - H_X, footPosition[1] - H_Y,
                    footPosition[2]);
    break;
  case 2: // RR
    return Vector3d(footPosition[0] + H_X, footPosition[1] + H_Y,
                    footPosition[2]);
    break;
  case 3: // LR
    return Vector3d(footPosition[0] + H_X, footPosition[1] - H_Y,
                    footPosition[2]);
    break;
  }
}

Vector3d legToBody(int legNumber, Vector3d footPosition) {
  switch (legNumber) {
  case 0: // RF
    return Vector3d(footPosition[0] + H_X, footPosition[1] - H_Y,
                    footPosition[2]);
    break;
  case 1: // LF
    return Vector3d(footPosition[0] + H_X, footPosition[1] + H_Y,
                    footPosition[2]);
    break;
  case 2: // RR
    return Vector3d(footPosition[0] - H_X, footPosition[1] - H_Y,
                    footPosition[2]);
    break;
  case 3: // LR
    return Vector3d(footPosition[0] - H_X, footPosition[1] + H_Y,
                    footPosition[2]);
    break;
  }
}

Matrix3d calculateJacobian(int legNumber, Vector3d jointPosition) {
  double gamma = jointPosition[0];
  double alfa = jointPosition[1];
  double beta = jointPosition[2];

  double delta, ksi;
#ifdef TO_WORLD
  delta = (legNumber == 0 || legNumber == 1) ? 1 : -1;
  ksi = (legNumber == 1 || legNumber == 3) ? 1 : -1;
#else
  delta = 0;
  ksi = (legNumber == 1 || legNumber == 3) ? 1 : -1;
#endif

  double pow_L23 = pow(L2, 2) + pow(L3, 2) - 2 * L2 * L3 * cos(beta);

  double x = L2 * cos(alfa) - L3 * cos(alfa + beta) + delta * H_X;
  double y = ksi * sqrt(pow_L23 - pow(x, 2)) * cos(gamma) +
             ksi * L1 * sin(gamma) + ksi * H_Y;
  double z = -sqrt(pow_L23 - pow(x, 2)) * sin(gamma) + L1 * cos(gamma);

  // Initialize Jacobian matrix
  Matrix3d J(3, 3);

  // Compute partial derivatives
  // For x
  J(0, 0) = -L2 * sin(alfa) + L3 * sin(alfa + beta);
  J(0, 1) = -L2 * cos(alfa) + L3 * cos(alfa + beta);
  J(0, 2) = delta * 0; // No beta dependency in x

  // For y
  J(1, 0) = -ksi * (L2 * sin(alfa) - L3 * sin(alfa + beta)) * sin(gamma);
  J(1, 1) = ksi * (L2 * cos(alfa) - L3 * cos(alfa + beta)) * sin(gamma);
  J(1, 2) = ksi * (L3 * cos(alfa + beta)) * sin(gamma);

  // For z
  J(2, 0) = -ksi * (L2 * sin(alfa) - L3 * sin(alfa + beta)) * cos(gamma);
  J(2, 1) = ksi * (L2 * cos(alfa) - L3 * cos(alfa + beta)) * cos(gamma);
  J(2, 2) = ksi * (L3 * cos(alfa + beta)) * cos(gamma) + L1 * sin(gamma);

  return J;
}

// #define time_print  //需要打印系统时间时保留

// /*
//     时间测量函数
//     用法：
//         #include <timetest.cpp>

//         float tStart = timetest();
//         //待测量代码段
//         float tEnd = timetest();
//         cout<<tEnd-tStart;
// */

// float timetest()
// {
//     auto now = std::chrono::system_clock::now();
//     // 通过不同精度获取相差的毫秒数
//     uint64_t dis_millseconds =
//     std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count()
//     -
//     std::chrono::duration_cast<std::chrono::seconds>(now.time_since_epoch()).count()
//     * 1000;
//     time_t tt = std::chrono::system_clock::to_time_t(now);
//     auto time_tm = localtime(&tt);
//     char strTime[25] = {0};
//     float ttt = time_tm->tm_hour * 3600 + time_tm->tm_min * 60 +
//     time_tm->tm_sec + 0.001*(float)dis_millseconds;
//     #ifdef time_print
//     sprintf(strTime, "%d-%02d-%02d %02d:%02d:%02d %03d", time_tm->tm_year +
//     1900, time_tm->tm_mon + 1, time_tm->tm_mday, time_tm->tm_hour,
//     time_tm->tm_min, time_tm->tm_sec, (int)dis_millseconds);
//     std::cout << strTime << std::endl;
//     #else
//     #endif

//     return ttt;
// }